#include "tex/tex_bitmap.h"
#include "res/res_font.h"
#include "frame.h"
#include "render/r_frustum.h"
#include "file/f_stream.h"
#include "tex/tex_bmp.h"
#include "text.h"
#include "vbo.h"

G3::FONT Font1;
G3::H_TEXTURE Texture1;
G3::TEXTBOX TextBox, TextBox2;

G3::FRAME frame (640, 480, 24, 0);

G3::FPS_COUNTER FPSCounter;
bool sprite_mode = true;
bool shader_mode = true;
bool m_grid = true;
bool m_render_bounding = true;
int m_cloud_id = -1;
int m_nodes_rendered = 0;
//int psize = 16;
float m_splat_size = 0.0f;

class PLY {
    public:
        PLY() {
            m_num_verts = 0;
            m_max_psize = 1000;
            m_size = 16;
            m_point_limit = 200;
        }
        ~PLY() {
            m_vbo.unbind();
        }

        class CLOUD {
            public:
                CLOUD() {}
                CLOUD( const CLOUD &r_cloud ) {
                    m_vertices = r_cloud.m_vertices;
                    m_normals = r_cloud.m_normals;
                    m_indices = r_cloud.m_indices;
                }
                ~CLOUD() {
                    m_vbo.unbind();
                }

                CLOUD &operator=( const CLOUD &r_cloud ) {
                    m_vertices = r_cloud.m_vertices;
                    m_normals = r_cloud.m_normals;
                    m_indices = r_cloud.m_indices;
                    return *this;
                }

                CLOUD &operator+=( const CLOUD &r_cloud ) {
                    size_t i;
                    for (i=0; i<r_cloud.m_vertices.size(); i++) {
                        m_vertices.push_back( r_cloud.m_vertices [i] );
                    }
                    for (i=0; i<r_cloud.m_normals.size(); i++) {
                        m_normals.push_back( r_cloud.m_normals [i] );
                    }
                    for (i=0; i<r_cloud.m_indices.size(); i++) {
                        m_indices.push_back( m_indices.size() );
                    }
                    return *this;
                }

                inline size_t size() const {
                    return m_vertices.size();
                }

                void clear() {
                    m_vertices.clear();
                    m_normals.clear();
                    m_indices.clear();
                }

                std::vector< G3::VEC3F > m_vertices;
                std::vector< G3::VEC3F > m_normals;
                std::vector< uint > m_indices;
                VBO m_vbo;
        };

        class NORMAL_CONE {
            public:
                NORMAL_CONE() {
                    m_angle = 0.0f;
                }

                NORMAL_CONE &operator=( const NORMAL_CONE &r_cone ) {
                    m_avg_normal = r_cone.m_avg_normal;
                    m_angle = r_cone.m_angle;
                    return *this;
                }

                G3::VEC3F m_avg_normal;
                float m_angle;

                void expand( const G3::VEC3F &r_normal ) {
                    if (r_normal.GetRootLength() < 1.0f)
                        return;
                    if (m_avg_normal.GetRootLength() < 1.0f) {
                        m_avg_normal = r_normal;
                    } else {
                        m_angle = ( m_angle + G3_ABS( G3::GetAngle( m_avg_normal, r_normal ) ) ) / 2.0f;
                        m_avg_normal += r_normal;
                    }
                }

                inline void normalize() {
                    m_avg_normal.Normalize();
                }
        };

        class NODE {
            public:
                NODE() {
                    m_children [0] = -1;
                    m_children [1] = -1;
                    m_avg_dist = 0.0f;
                }

                NODE &operator=( const NODE &r_node ) {
                    m_avg_dist = r_node.m_avg_dist;
                    m_children [0] = r_node.m_children [0];
                    m_children [1] = r_node.m_children [1];
                    m_bounding = r_node.m_bounding;
                    m_normal_cone = r_node.m_normal_cone;
                    return *this;
                }

                float m_avg_dist;
                int m_children [2];

                G3::AABB3F m_bounding;
                NORMAL_CONE m_normal_cone;
        };

        long m_num_verts;
        float m_max_psize;
        float m_size;

        std::vector< G3::VEC3F > m_vertices;
        std::vector< G3::VEC3F > m_normals;
        std::vector< uint > m_face_indices;
        std::vector< uint > m_indices;

        std::vector< CLOUD > m_clouds;
        std::vector< NODE > m_nodes;
        G3::AABB3F m_bounding;
        int m_point_limit;

        G3::H_TEXTURE m_texture;
        G3::MATERIAL m_material;
        G3::VBO m_vbo;

        void test() {
            m_point_limit = 1;

            m_num_verts = 4;

            m_indices.push_back( m_vertices.size() );
            m_vertices.push_back( G3::VEC3F (1, 1, 0) );
            m_normals.push_back( G3::VEC3F (-1, 0, 0) );

            m_indices.push_back( m_vertices.size() );
            m_vertices.push_back( G3::VEC3F (2, 1, 0) );
            m_normals.push_back( G3::VEC3F (1, 0, 0) );

            m_indices.push_back( m_vertices.size() );
            m_vertices.push_back( G3::VEC3F (1.5, 0.5, 0.5) );
            m_normals.push_back( G3::VEC3F (0, 0, 1) );

            m_indices.push_back( m_vertices.size() );
            m_vertices.push_back( G3::VEC3F (1.5, 1.5, 0.5) );
            m_normals.push_back( G3::VEC3F (0, 0, -1) );

            m_bounding.Minimum = m_vertices [0];
            m_bounding.Maximum = m_vertices [0];
            m_bounding.Expand( m_vertices [1]);
            m_bounding.Expand( m_vertices [2]);
            m_bounding.Expand( m_vertices [3]);
        }

        bool import( const std::string &s_filename ) {
            FILE *fi = fopen( s_filename.c_str(), "rt" );
            G3::TOKENLIST tokl;
            bool vertmode = false;
            bool facemode = false;
            char line [256];
            size_t line_no = 0;

            if (fi) {
                G3::MainLog.Report( "Loading PLY \"%s\"..\n", s_filename.c_str() );

                while( !feof( fi ) ) {
                    line_no++;

                    fscanf( fi, "%[^\n]\n", line );

                    tokl = Tokenize( line, " \t\r\n" );

                    if (!vertmode) {
                        if (tokl.size() >= 3 && tokl [0] == "element" && tokl [1] == "vertex") {
                            m_num_verts = G3::StrToInteger( tokl [2] );
                            m_vertices.reserve( m_num_verts );
                        } if (tokl.size() >= 1 && tokl [0] == "end_header")
                            vertmode = true;
                    } else if (!facemode) {
                        if (m_vertices.size() < (size_t) m_num_verts && tokl.size() >= 3) {
                            m_indices.push_back( m_vertices.size() );
                            m_vertices.push_back( G3::VEC3F( G3::StrToFloat( tokl [0] ), G3::StrToFloat( tokl [1] ), G3::StrToFloat( tokl [2] ) ) );
                            // First?
                            if (m_vertices.size() < 2) {
                                m_bounding.Minimum = m_vertices [0];
                                m_bounding.Maximum = m_vertices [0];
                            } else {
                                m_bounding.Expand( m_vertices [m_vertices.size() - 1] );
                            }
                        } else if (m_vertices.size() >= (size_t) m_num_verts) {
                            facemode = true;
                            G3::MainLog.Report( "Face indexes start at line %d\n", line_no );
                        } else {
                            break;
                        }
                    }
                    if (facemode) {
                        if (tokl.size() >= 4) {
                            m_face_indices.push_back( G3::StrToInteger( tokl [1] ) );
                            m_face_indices.push_back( G3::StrToInteger( tokl [2] ) );
                            m_face_indices.push_back( G3::StrToInteger( tokl [3] ) );
                        } else {
                            break;
                        }
                    }
                }

                fclose( fi );

                G3::MainLog.Report( "PLY loaded, calculating vertex normals..\n" );

                for (size_t i=0; i<m_vertices.size(); i++) {
                    G3::VEC3F face_normal;
                    G3::VEC3F normal;

                    for (size_t f=0; f<m_face_indices.size(); f+=3) {
                        face_normal = G3::GetNormal( m_vertices [m_face_indices [f]],
                                                     m_vertices [m_face_indices [f + 1]],
                                                     m_vertices [m_face_indices [f + 2]] );

                        if (m_face_indices [f] == i)
                            normal += face_normal;
                        if (m_face_indices [f + 1] == i)
                            normal += face_normal;
                        if (m_face_indices [f + 2] == i)
                            normal += face_normal;
                    }

                    // Unused vertex? Mark it as invalid
                    if (normal.GetLength() < G3_EPSILON)
                        normal.x = 3.0f;
                    else
                        normal.Normalize();

                    G3::MainLog.Report( "Vertex (%f %f %f) Normal (%f %f %f)\n",
                                        m_vertices [i].x, m_vertices [i].y, m_vertices [i].z,
                                        normal.x, normal.y, normal.z );

                    m_normals.push_back( normal );
                }

                G3::MainLog.Report( "Done with %d verts and %d normals, %d face indexes\n", m_vertices.size(), m_normals.size(),
                                    m_face_indices.size() );

                return true;
            }
            return false;
        }

        void dump( const std::string &s_filename ) {
            FILE *fo = fopen( s_filename.c_str(), "wt" );

            if (fo) {
                fprintf( fo, "Nodes: %d\nClouds: %d\n-------\n", m_nodes.size(), m_clouds.size() );

                for (size_t i=0; i<m_nodes.size(); i++) {
                    fprintf( fo, "Node %d:\n Avg Dist: %f\n Children: %d %d\n AABB: (%f %f %f) (%f %f %f)\n Normal cone: (%f %f %f) %f\n",
                             i, m_nodes [i].m_avg_dist, m_nodes [i].m_children [0], m_nodes [i].m_children [1],
                             m_nodes [i].m_bounding.Minimum.x, m_nodes [i].m_bounding.Minimum.y, m_nodes [i].m_bounding.Minimum.z,
                             m_nodes [i].m_bounding.Maximum.x, m_nodes [i].m_bounding.Maximum.y, m_nodes [i].m_bounding.Maximum.z,
                             m_nodes [i].m_normal_cone.m_avg_normal.x, m_nodes [i].m_normal_cone.m_avg_normal.y,
                             m_nodes [i].m_normal_cone.m_avg_normal.z, m_nodes [i].m_normal_cone.m_angle );
                }

                fprintf( fo, "-------\n" );

                for (size_t i=0; i<m_clouds.size(); i++) {
                    fprintf( fo, "Cloud %d:\n", i );
                    for (size_t j=0; j<m_clouds [i].m_vertices.size(); j++) {
                        fprintf( fo, " (%f %f %f) (%f %f %f)\n", m_clouds [i].m_vertices [j].x,
                                 m_clouds [i].m_vertices [j].y, m_clouds [i].m_vertices [j].z,
                                 m_clouds [i].m_normals [j].x, m_clouds [i].m_normals [j].y,
                                 m_clouds [i].m_normals [j].z );
                    }
                }

                fprintf( fo, "-------\n" );
            }

            fclose( fo );
        }

        void dump_test_avg_dist( const std::string &s_filename ) {
            std::vector< G3::VEC3F >::iterator it;
            FILE *fo = fopen( s_filename.c_str(), "wt" );

            if (fo) {
                VEC3F point;
                float min_dist;
                int counter;

                for (size_t i=0; i<m_vertices.size(); i+=m_point_limit) {
                    min_dist = 99999.0f;
                    counter = 0;

                    // Randomly select points for the node
                    it = m_vertices.begin();
                    while (it != m_vertices.end()) {
                        if (i == 0 || rand() % (m_vertices.size() / i) == 0) {
                            if (counter == 0)
                                point = *it;
                            else
                                min_dist = G3_MIN( min_dist, G3::GetDistance( point, *it ) );
                            counter++;
                        }
                        it++;
                    }

                    fprintf( fo, "%ld %f\n", i, min_dist );
                }
            }

            fclose( fo );
        }

        void load( const std::string &s_filename ) {
            FILE *fi = fopen( s_filename.c_str(), "rb" );

            if (fi) {
                char magic [4] = {'P','C','0','4'};
                size_t num_nodes = 0;
                size_t num_clouds = 0;
                size_t num_verts = 0;
                NODE node;
                CLOUD cloud;
                G3::VEC3F point, normal;

                fread( &magic, 4 * sizeof( char ), 1, fi );

                if (magic [0] != 'P' || magic [1] != 'C') {
                    G3::MainLog.Report( "\"%s\" is not a valid PointCloud file..\n", s_filename.c_str() );
                    return;
                }
                if (magic [2] != '0' || magic [3] != '4') {
                    G3::MainLog.Report( "\"%s\" is a PointCloud file with incompatible version..\n", s_filename.c_str() );
                    return;
                }

                fread( &num_nodes, sizeof( num_nodes ), 1, fi );
                fread( &num_clouds, sizeof( num_clouds ), 1, fi );

                for (size_t i=0; i<num_nodes; i++) {
                    fread( &node.m_avg_dist, sizeof( node.m_avg_dist ), 1, fi );
                    fread( &node.m_children [0], sizeof( node.m_children [0] ), 1, fi );
                    fread( &node.m_children [1], sizeof( node.m_children [1] ), 1, fi );
                    fread( &node.m_bounding, sizeof( node.m_bounding ), 1, fi );
                    fread( &node.m_normal_cone, sizeof( node.m_normal_cone ), 1, fi );

                    m_nodes.push_back( node );
                }

                for (size_t i=0; i<num_clouds; i++) {
                    m_clouds.push_back( cloud );

                    fread( &num_verts, sizeof( num_verts ), 1, fi );

                    for (size_t j=0; j<num_verts; j++) {
                        fread( &point, sizeof( point ), 1, fi );
                        fread( &normal, sizeof( normal ), 1, fi );

                        m_clouds [m_clouds.size() - 1].m_vertices.push_back( point );
                        m_clouds [m_clouds.size() - 1].m_normals.push_back( normal );
                        m_clouds [m_clouds.size() - 1].m_indices.push_back( j );
                    }
                }
            }

            fclose( fi );
        }

        // TODO:: Save as spheres, not AABB-s (takes less memory). Rethink Sphere intersection checks.
        // Or perhaps reconstruct AABB-s from bounding spheres..
        void save( const std::string &s_filename ) {
            FILE *fo = fopen( s_filename.c_str(), "wb" );

            if (fo) {
                char magic [4] = {'P','C','0','4'};
                size_t num_nodes = m_nodes.size();
                size_t num_clouds = m_clouds.size();
                size_t num_verts = 0;

                fwrite( &magic, 4 * sizeof( char ), 1, fo );
                fwrite( &num_nodes, sizeof( num_nodes ), 1, fo );
                fwrite( &num_clouds, sizeof( num_clouds ), 1, fo );

                for (size_t i=0; i<m_nodes.size(); i++) {
                    fwrite( &m_nodes [i].m_avg_dist, sizeof( m_nodes [i].m_avg_dist ), 1, fo );
                    fwrite( &m_nodes [i].m_children [0], sizeof( m_nodes [i].m_children [0] ), 1, fo );
                    fwrite( &m_nodes [i].m_children [1], sizeof( m_nodes [i].m_children [1] ), 1, fo );
                    fwrite( &m_nodes [i].m_bounding, sizeof( m_nodes [i].m_bounding ), 1, fo );
                    fwrite( &m_nodes [i].m_normal_cone, sizeof( m_nodes [i].m_normal_cone ), 1, fo );
                }

                for (size_t i=0; i<m_clouds.size(); i++) {
                    num_verts = m_clouds [i].size();

                    fwrite( &num_verts, sizeof( num_verts ), 1, fo );

                    for (size_t j=0; j<num_verts; j++) {
                        fwrite( &m_clouds [i].m_vertices [j], sizeof( m_clouds [i].m_vertices [j] ), 1, fo );
                        fwrite( &m_clouds [i].m_normals [j], sizeof( m_clouds [i].m_normals [j] ), 1, fo );
                    }
                }
            }

            fclose( fo );
        }

        class ITERATION_OBJ {
            public:
                ITERATION_OBJ() {
                    m_parent = 0;
                    m_depth = 0;
                }
                ITERATION_OBJ( const ITERATION_OBJ &r_obj ) {
                    m_parent = r_obj.m_parent;
                    m_depth = r_obj.m_depth;
                    m_bounding = r_obj.m_bounding;
                    m_cloud = r_obj.m_cloud;
                }

                int m_parent;
                int m_depth;
                G3::AABB3F m_bounding;
                CLOUD m_cloud;

                ITERATION_OBJ& operator=( const ITERATION_OBJ &r_obj ) {
                    m_parent = r_obj.m_parent;
                    m_depth = r_obj.m_depth;
                    m_bounding = r_obj.m_bounding;
                    m_cloud = r_obj.m_cloud;
                    return *this;
                }
        };

        void convert( float f_avg_dist ) {
            std::vector< G3::VEC3F >::iterator it;
            std::vector< G3::VEC3F >::iterator itn;
            NODE node [2];
            std::list< ITERATION_OBJ > iter_queue;
            std::list< ITERATION_OBJ >::iterator ii;
            ITERATION_OBJ iter;

            if (m_vertices.size() != m_normals.size() && !m_vertices.empty()) {
                G3::MainLog.Report( "Invalid number of vertex normals..\n" );
                return;
            }

            node [0].m_bounding = m_bounding;
            iter.m_bounding = m_bounding;
            node [0].m_avg_dist = f_avg_dist * sqrtf( m_vertices.size() / m_point_limit );

            // Push the root node
            m_nodes.push_back( node [0] );

            m_nodes [m_nodes.size () - 1].m_normal_cone.m_avg_normal = m_normals [0];

            m_clouds.push_back( iter.m_cloud );

            G3::MainLog.Report( "Selecting points for the root node..\n" );

            int part_taken = m_vertices.size() / m_point_limit;

            // Randomly select points for the node
            it = m_vertices.begin();
            itn = m_normals.begin();
            while (it != m_vertices.end()) {
                // Take half (or all, when there are less than m_point_limit of points)
                if ((part_taken > 1 && rand() % part_taken == 0) || part_taken <= 0) {
                    // Push it into the cloud of the node
                    m_clouds [m_clouds.size () - 1].m_vertices.push_back( *it );
                    m_clouds [m_clouds.size () - 1].m_normals.push_back( *itn );
                } else {
                    // Take the rest and store it for children nodes
                    iter.m_cloud.m_vertices.push_back( *it );
                    iter.m_cloud.m_normals.push_back( *itn );
                }

                m_nodes [m_nodes.size () - 1].m_normal_cone.expand( *itn );

                it++;
                itn++;
            }

            m_nodes [m_nodes.size () - 1].m_normal_cone.normalize();

            G3::MainLog.Report( "Recursing..\n" );

            // And subdivide it - if needed
            if (iter.m_cloud.size() != 0) {
                iter_queue.push_back( iter );
                ii = iter_queue.begin();

                while ( ii != iter_queue.end() ) {
                    G3::AABB3F bounding [2];
                    CLOUD cloud [2];
                    size_t id0 = 0, id1 = 0;
                    int part_taken = 0;
                    float avg_dist = 0.0f;

                    if (ii->m_cloud.m_vertices.size() == ii->m_cloud.m_normals.size()) {
                        // Create empty clouds to be filled with data later
                        id0 = m_clouds.size();
                        m_clouds.push_back( cloud [0] );
                        id1 = m_clouds.size();
                        m_clouds.push_back( cloud [1] );

                        G3::MainLog.Report( "Splitting bounding box..\n" );

                        // Split the bounding box
                        switch( ii->m_bounding.LongestAxis() ) {
                            default:
                            // X - axis is the longest
                            case 0:
                                bounding [0] = G3::AABB3F( ii->m_bounding.Minimum,
                                                           G3::VEC3F( 0.5f * ( ii->m_bounding.Minimum.x + ii->m_bounding.Maximum.x ),
                                                                      ii->m_bounding.Maximum.y, ii->m_bounding.Maximum.z ));
                                bounding [1] = G3::AABB3F( G3::VEC3F( 0.5f * ( ii->m_bounding.Minimum.x + ii->m_bounding.Maximum.x ),
                                                                      ii->m_bounding.Minimum.y, ii->m_bounding.Minimum.z ),
                                                           ii->m_bounding.Maximum );
                                break;
                            // Y - axis is the longest
                            case 1:
                                bounding [0] = G3::AABB3F( ii->m_bounding.Minimum,
                                                           G3::VEC3F( ii->m_bounding.Maximum.x,
                                                                      0.5f * ( ii->m_bounding.Minimum.y + ii->m_bounding.Maximum.y ),
                                                                      ii->m_bounding.Maximum.z ));
                                bounding [1] = G3::AABB3F( G3::VEC3F( ii->m_bounding.Minimum.x,
                                                                      0.5f * ( ii->m_bounding.Minimum.y + ii->m_bounding.Maximum.y ),
                                                                      ii->m_bounding.Minimum.z ),
                                                           ii->m_bounding.Maximum );
                                break;
                            // Z - axis is the longest
                            case 2:
                                bounding [0] = G3::AABB3F( ii->m_bounding.Minimum,
                                                           G3::VEC3F( ii->m_bounding.Maximum.x,
                                                                      ii->m_bounding.Maximum.y,
                                                                      0.5f * ( ii->m_bounding.Minimum.z + ii->m_bounding.Maximum.z ) ) );
                                bounding [1] = G3::AABB3F( G3::VEC3F( ii->m_bounding.Minimum.x,
                                                                      ii->m_bounding.Minimum.y,
                                                                      0.5f * ( ii->m_bounding.Minimum.z + ii->m_bounding.Maximum.z ) ),
                                                           ii->m_bounding.Maximum );
                                break;
                        }

                        node [0].m_bounding = bounding [0];
                        node [1].m_bounding = bounding [1];

                        G3::MainLog.Report( "Splitting point cloud..\n" );

                        part_taken = ii->m_cloud.size() / m_point_limit;

                        // Split the cloud between the two children
                        it = ii->m_cloud.m_vertices.begin();
                        itn = ii->m_cloud.m_normals.begin();
                        while (it != ii->m_cloud.m_vertices.end()) {
                            // First child?
                            if ( node [0].m_bounding.PointInside( *it ) ) {
                                // Take half
                                if ((part_taken > 1 && rand() % part_taken == 0) || part_taken <= 0) {
                                    m_clouds [id0].m_vertices.push_back( *it );
                                    m_clouds [id0].m_normals.push_back( *itn );
                                } else {
                                    // Take the rest and store it for grandchildren
                                    cloud [0].m_vertices.push_back( *it );
                                    cloud [0].m_normals.push_back( *itn );
                                }

                                if (itn == ii->m_cloud.m_normals.begin())
                                    node [0].m_normal_cone.m_avg_normal = *itn;
                                else
                                    node [0].m_normal_cone.expand( *itn );
                            // Second child?
                            } else {
                                // Take half
                                if ((part_taken > 1 && rand() % part_taken == 0) || part_taken <= 0) {
                                    m_clouds [id1].m_vertices.push_back( *it );
                                    m_clouds [id1].m_normals.push_back( *itn );
                                } else {
                                    // Take the rest and store it for grandchildren
                                    cloud [1].m_vertices.push_back( *it );
                                    cloud [1].m_normals.push_back( *itn );
                                }

                                if (itn == ii->m_cloud.m_normals.begin())
                                    node [1].m_normal_cone.m_avg_normal = *itn;
                                else
                                    node [1].m_normal_cone.expand( *itn );
                            }

                            it++;
                            itn++;
                        }

                        node [0].m_normal_cone.normalize();
                        node [1].m_normal_cone.normalize();

                        // This formula is experimental and was fitted from simple statistics
                        //avg_dist = 3.7f / ( ii->m_depth * m_point_limit ) + 0.001067f;
                        avg_dist = ( m_vertices.size() * 0.00009661f + 0.227f ) / ( ii->m_depth * m_point_limit ) + f_avg_dist;

                        node [0].m_avg_dist = avg_dist;
                        node [1].m_avg_dist = avg_dist;

                        // Set parent children id-s
                        m_nodes [ii->m_parent].m_children [0] = m_nodes.size();
                        m_nodes [ii->m_parent].m_children [1] = m_nodes.size() + 1;
                        // And push the nodes
                        m_nodes.push_back( node [0] );
                        m_nodes.push_back( node [1] );

                        G3::MainLog.Report( "Recursing..\n" );
                        G3::MainLog.Report( "PClouds with (%d, %d) (%d, %d)..\n", m_clouds [id0].size(), cloud [0].size(), m_clouds [id1].size(), cloud [1].size() );

                        // Recurse until the point limit is reached
                        if (cloud [0].size() != 0 && m_clouds [id0].size() + cloud [0].size() > (size_t) m_point_limit) {
                            iter.m_bounding = bounding [0];
                            iter.m_parent = m_nodes [ii->m_parent].m_children [0];
                            iter.m_cloud = cloud [0];
                            iter.m_depth = ii->m_depth + 1;

                            iter_queue.push_back( iter );
                        } else {
                            // Also add the rest of the cloud points
                            m_clouds [id0] += cloud [0];
                        }
                        if (cloud [0].size() != 0 && m_clouds [id1].size() + cloud [1].size() > (size_t) m_point_limit) {
                            iter.m_bounding = bounding [1];
                            iter.m_parent = m_nodes [ii->m_parent].m_children [1];
                            iter.m_cloud = cloud [1];
                            iter.m_depth = ii->m_depth + 1;

                            iter_queue.push_back( iter );
                        } else {
                            // Also add the rest of the cloud points
                            m_clouds [id1] += cloud [1];
                        }
                    } else {
                        G3::MainLog.Report( "Invalid number of cloud vertex normals..\n" );
                    }

                    ii = iter_queue.erase( ii );
                }
            }
        }
/*
        // TODO:: Iterative version - stack is too small for large point clouds
        void recurse( int i_parent, G3::AABB3F &r_bounding, CLOUD &r_cloud, float f_avg_dist ) {
            static int i_depth = 0;
            std::vector< G3::VEC3F >::iterator it;
            std::vector< G3::VEC3F >::iterator itn;
            G3::AABB3F bounding [2];
            NODE node [2];
            CLOUD cloud [2];

            if (r_cloud.m_vertices.size() != r_cloud.m_normals.size()) {
                G3::MainLog.Report( "Invalid number of cloud vertex normals..\n" );
                return;
            }

            i_depth++;

            // Create empty clouds to be filled with data later
            size_t id0 = m_clouds.size();
            m_clouds.push_back( cloud [0] );
            size_t id1 = m_clouds.size();
            m_clouds.push_back( cloud [1] );

            G3::MainLog.Report( "Splitting bounding box..\n" );

            // Split the bounding box
            switch( r_bounding.LongestAxis() ) {
                default:
                // X - axis is the longest
                case 0:
                    bounding [0] = G3::AABB3F( r_bounding.Minimum,
                                               G3::VEC3F( 0.5f * ( r_bounding.Minimum.x + r_bounding.Maximum.x ),
                                                          r_bounding.Maximum.y, r_bounding.Maximum.z ));
                    bounding [1] = G3::AABB3F( G3::VEC3F( 0.5f * ( r_bounding.Minimum.x + r_bounding.Maximum.x ),
                                                          r_bounding.Minimum.y, r_bounding.Minimum.z ),
                                               r_bounding.Maximum );
                    break;
                // Y - axis is the longest
                case 1:
                    bounding [0] = G3::AABB3F( r_bounding.Minimum,
                                               G3::VEC3F( r_bounding.Maximum.x,
                                                          0.5f * ( r_bounding.Minimum.y + r_bounding.Maximum.y ),
                                                          r_bounding.Maximum.z ));
                    bounding [1] = G3::AABB3F( G3::VEC3F( r_bounding.Minimum.x,
                                                          0.5f * ( r_bounding.Minimum.y + r_bounding.Maximum.y ),
                                                          r_bounding.Minimum.z ),
                                               r_bounding.Maximum );
                    break;
                // Z - axis is the longest
                case 2:
                    bounding [0] = G3::AABB3F( r_bounding.Minimum,
                                               G3::VEC3F( r_bounding.Maximum.x,
                                                          r_bounding.Maximum.y,
                                                          0.5f * ( r_bounding.Minimum.z + r_bounding.Maximum.z ) ) );
                    bounding [1] = G3::AABB3F( G3::VEC3F( r_bounding.Minimum.x,
                                                          r_bounding.Minimum.y,
                                                          0.5f * ( r_bounding.Minimum.z + r_bounding.Maximum.z ) ),
                                               r_bounding.Maximum );
                    break;
            }

            node [0].m_bounding = bounding [0];
            node [1].m_bounding = bounding [1];

            G3::MainLog.Report( "Splitting point cloud..\n" );

            int part_taken = r_cloud.size() / m_point_limit;

            // Split the cloud between the two children
            it = r_cloud.m_vertices.begin();
            itn = r_cloud.m_normals.begin();
            while (it != r_cloud.m_vertices.end()) {
                // First child?
                if ( node [0].m_bounding.PointInside( *it ) ) {
                    // Take half
                    if ((part_taken > 1 && rand() % part_taken == 0) || part_taken <= 0) {
                        m_clouds [id0].m_vertices.push_back( *it );
                        m_clouds [id0].m_normals.push_back( *itn );
                    } else {
                        // Take the rest and store it for grandchildren
                        cloud [0].m_vertices.push_back( *it );
                        cloud [0].m_normals.push_back( *itn );
                    }

                    if (itn == r_cloud.m_normals.begin())
                        node [0].m_normal_cone.m_avg_normal = *itn;
                    else
                        node [0].m_normal_cone.expand( *itn );
                // Second child?
                } else {
                    // Take half
                    if ((part_taken > 1 && rand() % part_taken == 0) || part_taken <= 0) {
                        m_clouds [id1].m_vertices.push_back( *it );
                        m_clouds [id1].m_normals.push_back( *itn );
                    } else {
                        // Take the rest and store it for grandchildren
                        cloud [1].m_vertices.push_back( *it );
                        cloud [1].m_normals.push_back( *itn );
                    }

                    if (itn == r_cloud.m_normals.begin())
                        node [1].m_normal_cone.m_avg_normal = *itn;
                    else
                        node [1].m_normal_cone.expand( *itn );
                }

                it++;
                itn++;
            }

            node [0].m_normal_cone.normalize();
            node [1].m_normal_cone.normalize();

            // This formula is experimental and was fitted from simple statistics
            float avg_dist = 3.7f / ( i_depth * m_point_limit ) + 0.001067f;

            node [0].m_avg_dist = avg_dist;
            node [1].m_avg_dist = avg_dist;

            // Set parent children id-s
            m_nodes [i_parent].m_children [0] = m_nodes.size();
            m_nodes [i_parent].m_children [1] = m_nodes.size() + 1;
            // And push the nodes
            m_nodes.push_back( node [0] );
            m_nodes.push_back( node [1] );

            G3::MainLog.Report( "Recursing..\n" );
            G3::MainLog.Report( "PClouds with (%d, %d) (%d, %d)..\n", m_clouds [id0].size(), cloud [0].size(), m_clouds [id1].size(), cloud [1].size() );

            // Recurse until the point limit is reached
            if (cloud [0].size() != 0 && m_clouds [id0].size() + cloud [0].size() > m_point_limit) {
                recurse( m_nodes [i_parent].m_children [0], bounding [0], cloud [0], f_avg_dist );
            } else {
                // Also add the rest of the cloud points
                m_clouds [id0] += cloud [0];
            }
            if (cloud [0].size() != 0 && m_clouds [id1].size() + cloud [1].size() > m_point_limit) {
                recurse( m_nodes [i_parent].m_children [1], bounding [1], cloud [1], f_avg_dist );
            } else {
                // Also add the rest of the cloud points
                m_clouds [id1] += cloud [1];
            }
        }
*/
        void build() {
            // Loaded or imported?
            if (m_num_verts == 0 ) {
                // Build all clouds
                for (size_t i=0; i<m_clouds.size(); i++) {
                    if (m_clouds [i].m_vertices.empty()) {
                        G3::MainLog.Report( "Skipping VBO %d..\n", i );
                    } else {
                        G3::MainLog.Report( "Building VBO %d..\n", i );

                        m_clouds [i].m_vbo.init( m_clouds [i].size(), &m_clouds [i].m_vertices [0],
                                                 m_clouds [i].size(), &m_clouds [i].m_indices [0],
                                                 0, NULL, NULL,
                                                 &m_clouds [i].m_normals [0] );
                        m_clouds [i].m_vbo.build();
                    }
                }
            } else {
                G3::MainLog.Report( "Building VBO..\n" );

                m_vbo.init( m_num_verts, &m_vertices [0], m_num_verts, &m_indices [0], 0, NULL, NULL, &m_normals [0] );
                m_vbo.build();
                m_vbo.bind();
            }

            if (GLEE_ARB_point_parameters) {
                glGetFloatv (GL_POINT_SIZE_MAX_ARB, &m_max_psize);

                if (m_size > m_max_psize)
                    m_size = m_max_psize;
            }

            G3::MainLog.Report( "Loading texture..\n" );

            m_texture = G3::TextureManager.GetHandle( "textures/dot.jpg" );
            m_material.shader_program.AttachShader( "shaders/pcloud.vert", G3::SP_VERTEX );
            m_material.shader_program.AttachShader( "shaders/pcloud.frag", G3::SP_FRAGMENT );
            m_material.shader_program.Link();

            G3::MainLog.Report( "Done, ready for rendering..\n" );
        }

        void render_node( int i_node, const G3::FRUSTUM_CLASSIFIC &r_classific, bool just_this_cloud = false ) {
            boost::shared_ptr<SHADER_BASE> shader;
            G3::FRUSTUM_CLASSIFIC classific;
            G3::VEC3F cpoint;

            if (r_classific == G3::GFC_HALF) {
                classific = frame.Renderer.Camera.ViewFrustum.IsBoxIn( m_nodes [i_node].m_bounding );
                if (classific == G3::GFC_OUT)
                    return;
            } else if (r_classific == G3::GFC_OUT) {
                return;
            } else {
                classific = r_classific;
            }

            float angle = G3_ABS( G3::GetAngle( m_nodes [i_node].m_normal_cone.m_avg_normal, frame.Renderer.Camera.GetForwardVector() ) ) - G3_PI;

/*            if ( G3_ABS( angle ) > m_nodes [i_node].m_normal_cone.m_angle )
                return;*/

            // Find the average distance required for lossless rendering
            float fconst = tanf( G3_DEG2RAD ( frame.Renderer.Camera.ViewFrustum.FieldOfView / frame.Width ) );
            m_splat_size = G3::GetDistance( cpoint, frame.Renderer.Camera.Position ) * fconst;

            // Render this cloud
            if (!m_clouds [i_node].m_vertices.empty()) {
                if (!sprite_mode) {
                    if (shader_mode) {
                        glDisable( GL_TEXTURE_2D );
                        //glEnable( GL_TEXTURE_2D );
                        glColor4f(1, 1, 1, 1);

                        glEnable( GL_VERTEX_PROGRAM_POINT_SIZE );

                        m_material.glApply();

                        shader = m_material.shader_program.GetShader (G3::SP_VERTEX, 0);

                        if (shader) {
                            //shader->SetTexture2D (m_material.shader_program.programObject, "t_texture1", Texture1);
                            shader->SetFloat( m_material.shader_program.programObject, "f_avg_dist", m_nodes [i_node].m_avg_dist );
                            shader->SetFloat( m_material.shader_program.programObject, "f_fconst", fconst);
                        }
                    }

                    m_clouds [i_node].m_vbo.bind();

                    glDrawElements (GL_POINTS, m_clouds [i_node].size(), GL_UNSIGNED_INT, (void *) 0);

                    m_clouds [i_node].m_vbo.unbind();

                    if (shader_mode) {
                        m_material.glUnApply();

                        glDisable( GL_VERTEX_PROGRAM_POINT_SIZE );

                        glColor4f(1, 1, 1, 1);
                    }
                } else {
                    m_clouds [i_node].m_vbo.bind();

                    glDrawElements (GL_POINTS, m_clouds [i_node].size(), GL_UNSIGNED_INT, (void *) 0);

                    m_clouds [i_node].m_vbo.unbind();
                }

                m_nodes_rendered++;
            }

            // And recurse further - if necessary
            if (!just_this_cloud && m_nodes [i_node].m_avg_dist >= m_splat_size) {
                if (m_nodes [i_node].m_children [0] > 0 && (size_t) m_nodes [i_node].m_children [0] < m_nodes.size()) {
                    render_node( m_nodes [i_node].m_children [0], classific );
                } if (m_nodes [i_node].m_children [1] > 0 && (size_t) m_nodes [i_node].m_children [1] < m_nodes.size()) {
                    render_node( m_nodes [i_node].m_children [1], classific );
                }
            }
        }

        void render() {
            m_nodes_rendered = 0;

            if (sprite_mode) {
                glEnable (GL_POINT_SPRITE_ARB);
                // This is how our point sprite's size will be modified by
                // distance from the viewer.
                // 1/(a + b*d + c*d^2)
                float quadratic[] =  { 1.0f, 0.0f, 0.01f };
                glPointParameterfvARB (GL_POINT_DISTANCE_ATTENUATION_ARB, quadratic);

                // The alpha of a point is calculated to allow the fading of points
                // instead of shrinking them past a defined threshold size. The threshold
                // is defined by GL_POINT_FADE_THRESHOLD_SIZE_ARB and is not clamped to
                // the minimum and maximum point sizes.
                glPointParameterfARB (GL_POINT_FADE_THRESHOLD_SIZE_ARB, 60.0f);

                glPointParameterfARB (GL_POINT_SIZE_MIN_ARB, 1.0f);
                glPointParameterfARB (GL_POINT_SIZE_MAX_ARB, m_max_psize);

                // Specify point sprite texture coordinate replacement mode for each texture unit
                glTexEnvf (GL_POINT_SPRITE_ARB, GL_COORD_REPLACE_ARB, GL_TRUE);

                glEnable (GL_DEPTH_TEST);
                glDepthMask (GL_FALSE);

                glEnable (GL_BLEND);
                glBlendFunc (GL_SRC_ALPHA, GL_ONE);

                glEnable (GL_TEXTURE_2D);

                TextureManager.Apply (m_texture);
            } else {
                glDisable( GL_TEXTURE_2D );
                glColor4f(1, 1, 1, 1);

                glEnable (GL_POINT_SPRITE_ARB);

                // Specify point sprite texture coordinate replacement mode for each texture unit
//                glTexEnvf (GL_POINT_SPRITE_ARB, GL_COORD_REPLACE_ARB, GL_TRUE);
            }

            glPointSize (m_size);

            if (m_num_verts > 0) {
                // Render the whole model, when imported and not converted
                glDrawElements (GL_POINTS, m_num_verts, GL_UNSIGNED_INT, (void *) 0);
            } else {
//                float f = 0.0f;

                // Render all clouds, when none particular selected
                if (m_cloud_id < 0 || (size_t) m_cloud_id >= m_clouds.size()) {
                    if (!m_nodes.empty())
                        render_node( 0, G3::GFC_HALF );

/*                    if (m_render_bounding) {
                        for ( size_t i=0; i<m_nodes.size(); i++ ) {
                            f = i;
                            m_nodes [i].m_bounding.Render();
                        }
                    }*/
                // Render the selected cloud
                } else {
                    if (!m_clouds [m_cloud_id].m_vertices.empty()) {
                        render_node( m_cloud_id, G3::GFC_HALF, true );
                    }

/*                    if (m_render_bounding && m_cloud_id < m_nodes.size())
                        m_nodes [m_cloud_id].m_bounding.Render();*/
                }
            }

            if (sprite_mode) {
                glDisable (GL_POINT_SPRITE_ARB);

                glDepthMask (GL_TRUE);
                glDisable (GL_BLEND);
            } else {
                glDisable (GL_POINT_SPRITE_ARB);

                glColor4f(1, 1, 1, 1);
            }
        }
};

PLY bunny;

void PCUpdateHandler (G3::INPUT *Input, float aElapsedTime) {
	if (Input->Keyboard.IsKeyDown (SDLK_a))
		Input->Frame->Renderer.Camera.Move (G3::VEC3F (0.0f, 1.0f, 0.0f) * aElapsedTime);
	if (Input->Keyboard.IsKeyDown (SDLK_z))
		Input->Frame->Renderer.Camera.Move (G3::VEC3F (0.0f, -1.0f, 0.0f) * aElapsedTime);
	if (Input->Keyboard.IsKeyDown (SDLK_LEFT))
		Input->Frame->Renderer.Camera.Move (G3::VEC3F (-1.0f, 0.0f, 0.0f) * aElapsedTime);
	if (Input->Keyboard.IsKeyDown (SDLK_RIGHT))
		Input->Frame->Renderer.Camera.Move (G3::VEC3F (1.0f, 0.0f, 0.0f) * aElapsedTime);
	if (Input->Keyboard.IsKeyDown (SDLK_UP))
		Input->Frame->Renderer.Camera.Move (G3::VEC3F (0.0f, 0.0f, 1.0f) * aElapsedTime);
	if (Input->Keyboard.IsKeyDown (SDLK_DOWN))
		Input->Frame->Renderer.Camera.Move (G3::VEC3F (0.0f, 0.0f,-1.0f) * aElapsedTime);

	if (Input->Keyboard.IsKeyDown (SDLK_ESCAPE))
		Input->SendQuit ();
}

void PCKeyDownHandler (INPUT *Input, SDL_keysym* keysym, float aElapsedTime) {
    if (keysym->sym == SDLK_l) {
        bunny.load( "test2.pcl" );
        bunny.dump( "dump3.txt" );
        bunny.build();
    } else if (keysym->sym == SDLK_s) {
        bunny.save( "test2.pcl" );
    } else if (keysym->sym == SDLK_i) {
//        bunny.import( "../../media/pointclouds/bunny/reconstruction/bun_zipper.ply" );
        bunny.import( "../../media/pointclouds/bunny/reconstruction/bun_zipper_res2.ply" );
        bunny.build();
    } else if (keysym->sym == SDLK_t) {
        bunny.test();
        bunny.build();
    } else if (keysym->sym == SDLK_c) {
        bunny.convert( 0.001067f );
        bunny.dump( "dump2.txt" );
    } else if (keysym->sym == SDLK_p) {
        sprite_mode ^= 1;
    } else if (keysym->sym == SDLK_g) {
        m_grid ^= 1;
    } else if (keysym->sym == SDLK_j) {
        shader_mode ^= 1;
    } else if (keysym->sym == SDLK_PAGEUP) {
        m_cloud_id++;
    } else if (keysym->sym == SDLK_PAGEDOWN) {
        m_cloud_id--;
    } else if (keysym->sym == SDLK_b) {
        m_render_bounding ^= 1;
    } else if (keysym->sym == SDLK_e) {
        bunny.dump_test_avg_dist( "dists3.txt" );
    }
}

int main (int argc, char **argv) {
    frame.Create ("Game3ngine");

    frame.Renderer.Create (640, 480);

    frame.Input.Mouse.SetMode (MOUSE_CENTER);

    frame.Input.doOnUpdate (PCUpdateHandler);
    frame.Input.doOnKeyDown (PCKeyDownHandler);

    /* Enable smooth shading */
    glShadeModel( GL_SMOOTH );

    /* Set the background black */
    glClearColor( 0.0f, 0.0f, 0.0f, 0.0f );

    /* The Type Of Depth Test To Do */
    glDepthFunc( GL_LEQUAL );

    G3::MainFilesys.MountDir ("../../media/font", "");
    G3::MainFilesys.MountDir ("../../media", "");

    Font1.Load ("font.lua");
    TextBox.Font = &Font1;
    TextBox2.Font = &Font1;

    G3::MainLog.Report ("Font loaded..\n");

    TextBox.Position = G3::VEC2I (10, 10);
    TextBox2.Position = G3::VEC2I (10, 22);

    float ambient[4] = {0.2f, 0.2f, 0.2f, 1.0f};
    glLightModelfv (GL_LIGHT_MODEL_AMBIENT, ambient);

    float lightpos[4] = {3, 1, 3, 1};
    float lightcol[4] = {0.7f, 0.7f, 0.7f, 1};
    float lightspec[4] = {1, 1, 1, 1};
    glLightfv (GL_LIGHT0, GL_POSITION, lightpos);
/*    glLightfv (GL_LIGHT0, GL_DIFFUSE, lightcol);
    glLightfv (GL_LIGHT0, GL_SPECULAR, lightspec);*/

    glEnable (GL_LIGHTING);
    glEnable (GL_LIGHT0);

    glDisable (GL_COLOR_MATERIAL);

    float matcol[4] = {0.7f, 0.7f, 0.7f, 1};
    float matspec[4] = {0.8f, 0.9f, 1, 1};
    float matemit[4] = {0, 0, 0, 1};
    glMaterialf (GL_FRONT, GL_SHININESS, 15.0f);
/*    glMaterialfv (GL_FRONT, GL_AMBIENT_AND_DIFFUSE, matcol);
    glMaterialfv (GL_FRONT, GL_SPECULAR, matspec);
    glMaterialfv (GL_FRONT, GL_EMISSION, matemit);*/

//    G3::MainLog.Report("Testangle: %f\n", G3_RAD2DEG( G3::GetAngle( G3::VEC3F( 1, 0, 0 ), G3::VEC3F( cos( 135.0f * G3_PI / 180.0 ), sin( 135.0f * G3_PI / 180.0 ), 0 ) ) ) );
    Texture1 = G3::TextureManager.GetHandle ("textures/concrete001.jpg");

    FPSCounter.Start ();

    while (true) {
        frame.Input.GetInput ();

        frame.Renderer.ClearBuffer ();

        FPSCounter.CheckTime ();

        frame.Renderer.Look ();

        if (m_grid)
            frame.Renderer.RenderGrid ();

        bunny.render();

        TextBox.glPrint ("FPS: %.2f dt %.3f", FPSCounter.FPS, FPSCounter.LastDelta);
        TextBox2.glPrint ("Cloud %d Nodes %d Splat %f %f Shader %d", m_cloud_id, m_nodes_rendered, m_splat_size, 0.012 / m_splat_size, shader_mode);
        //TextBox2.glPrint ("Cloud %d Nodes %d Splat %f Shader %d PSize %d", m_cloud_id, m_nodes_rendered, m_splat_size, shader_mode, psize);

        frame.Renderer.SwapBuffer ();

        FPSCounter.AddFrame ();

        // Keep the main thread from consuming all the resources
        // TODO:: An automatic FPS regulation system needs to be devised
		SDL_Delay (10);
    }

    Font1.Clear ();

    return 0;
}
